#include "Scene.h"
#include <QPointF>
#include <QVector>
#include <QPolygonF>
#include "basic_typedef.h"
#include "Sampler.h"
#include "CollisionDetector.h"
#include "Prm.h"

class Planner
{
public:
	
	////////////////////////
	// C'tors & D'tors
	////////////////////////

	/*	The constructor transforms the input data (obstacles, query, and robot radius)
		from the GUI to CGAL objects. 
	*/
	Planner(Scene* scene): m_scene(scene), 
			m_radius(scene->getRadius()),
			m_robot_num(scene->getStartPositions().size())
	{
		// transform data from Scene
		QVector<QPolygonF> obstacles = m_scene->getObstacles();
		foreach(QPolygonF qp, obstacles)
		{
			Polygon_2 cp;
			for (int j=0; j < qp.size(); j++)
			{
				cp.push_back(transformToCGAL(qp[j]));
			}

			m_obstacles.push_back(cp);
		}
	
		QVector<QVector<QPointF>> startPositions = m_scene->getStartPositions();
		QVector<QVector<QPointF>> targetPositions = m_scene->getTargetPositions();

		m_start_point = transformToCGAL(startPositions[0][0]);
		m_target_point = transformToCGAL(targetPositions[0][0]);

		Point_2 ptl = transformToCGAL(m_scene->getRoomTopLeft());
		Point_2 pbr = transformToCGAL(m_scene->getRoomBottomRight());
		m_room = Room(ptl,pbr);
	}

	~Planner()
	{
		delete m_collision;
		delete m_sampler;
	}

	/*	This function is called by the GUI when you press the
		"Execute" button. 
		You can change the structure of Prm.h, but remember 
		to run the "tranform_path" function to update the gui	*/
	void run()
	{
		m_collision = new CollisionDetector(m_radius, &m_obstacles);
		m_sampler = new Sampler(m_radius, m_room, m_collision);
		
		// An example 
		Prm roadmap = Prm(300, 12, m_collision, 
						m_sampler, m_start_point, m_target_point);
		roadmap.generate_roadmap();

		// retrieve path from PRM
		vector<Point_2> path = roadmap.retrieve_path();

		// transform path to GUI and update the display in gui
		transfrom_path(path);
	}

private:
	/////////////////
	// Procedures that transform CGAL objects to GUI and vice versa
	/////////////////

	Point_2		transformToCGAL(QPointF qp)
	{
		// x and y coordinates of the point
		double x = qp.x();
		double y = qp.y();

		Point_2 cpoint(x,y);
		return cpoint;
	}

	QPointF		transformToQT(Point_2 cp)
	{
		return QPointF(cp.x() , cp.y() );
	}

	QVector<QPointF> transformToQT(PointSet cs)
	{
		QVector<QPointF> result;

		foreach(Point_2 c, cs)
		{
			result.push_back(transformToQT(c));
		}
		return result;
	}

	QVector<QVector<QPointF>> transformToQT(Path path)
	{
		QVector<QVector<QPointF>> result;

		foreach(PointSet cs, path)
		{
			result.push_back(transformToQT(cs));
		}
		return result;
	}

	void transfrom_path(vector<Point_2>& path)
	{
		vector<vector<Point_2>> multi_path;
		foreach(Point_2 p, path)
		{
			vector<Point_2> dummy;
			dummy.push_back(p);
			multi_path.push_back(dummy);
		}

		// path section number - group - info
			vector<vector<pair<QTPath,GeometricGraph>>> result;
		
			vector<pair<Point_2,Point_2>> dummyPairVector;
			PointSet		dummyCS;
			GeometricGraph dummyGG(dummyPairVector, dummyCS);

			foreach(PointSet cs, multi_path)
			{
				vector<pair<QTPath,GeometricGraph>> path_graph_section;
				vector<PointSet> strip_cs;
				foreach(Point_2 c, cs)
				{
					PointSet cs2;
					cs2.push_back(c);
					strip_cs.push_back(cs2);
				}

				foreach(PointSet group_cs, strip_cs)
				{
					list<PointSet> dummyPath;
					dummyPath.push_back(group_cs);
					QVector<QVector<QPointF>> qt_path = transformToQT(dummyPath);
					pair<QTPath,GeometricGraph> p(qt_path, dummyGG);
					path_graph_section.push_back(p);
				}
				result.push_back(path_graph_section);
			}

			m_scene->setPath(result);

	}

	double path_legth(vector<vector<Point_2>> path)
	{
		double length = 0;
		for (int i = 1; i < path.size(); i++)
		{
			for (int r = 0; r < m_robot_num; r++)
			{
				length += pow(path[i][r].x() - path[i-1][r].x(),2) + 
					pow(path[i][r].y() - path[i-1][r].y(),2);
			}
		}
		return length;
	}

	///////////////////////
	// Data members
	///////////////////////

	/*	Scene is the interface class between the GUI and the planner.
		The extraction of the scenario data is done through this 
		object. The result of the planner (the path) is returned
		using this object as well.	*/
	Scene*		m_scene;		
	double		m_radius;		// Radius of the robot
	Obstacles	m_obstacles;// Obstacles, represented by polygons
	int			m_robot_num;	// obsolete 
	Point_2		m_start_point, m_target_point; // Start/target configurations
	Room		m_room;	// Boundaries of the scenario

	CollisionDetector*	m_collision;    //	Collision detection black box
	Sampler*			m_sampler;		//	Sampler object
};


